#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <boost/asio.hpp>
#include <thread>
#include <string>
#include <sstream>
#include <iomanip>

using boost::asio::ip::udp;

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(ar) (sizeof(ar)/sizeof(ar[0]))
#endif

class GimbalControlBridgeNode : public rclcpp::Node {
public:
    GimbalControlBridgeNode()
    : Node("gimbal_control_bridge_node"),
      udp_ioc_(),
      serial_ioc_(),
      udp_socket_(udp_ioc_, udp::endpoint(udp::v4(), 0)),  // 初始化UDP socket，动态分配端口
      serial_port_(serial_ioc_),
      buffer_("") {

        // 声明和获取参数
        this->declare_parameter<std::string>("udp_ip", "192.168.144.108");
        this->declare_parameter<int>("udp_port", 5000);
        this->declare_parameter<std::string>("serial_port", "/dev/ttyS7");
        this->declare_parameter<int>("baud_rate", 230400);

        this->get_parameter("udp_ip", udp_ip_);
        this->get_parameter("udp_port", udp_port_);
        this->get_parameter("serial_port", serial_port_name_);
        this->get_parameter("baud_rate", baud_rate_);

        // 初始化UDP和串口
        remote_endpoint_ = udp::endpoint(boost::asio::ip::make_address(udp_ip_), udp_port_);
        open_serial_port();

        // 订阅速度控制消息
        speed_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "gimbal_speed_control", 10,
            std::bind(&GimbalControlBridgeNode::handle_speed_control, this, std::placeholders::_1));

        // 订阅角度控制消息
        angle_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            "gimbal_angle_control", 10,
            std::bind(&GimbalControlBridgeNode::handle_angle_control, this, std::placeholders::_1));

        // 创建ROS发布器，用于发布目标位置
        position_publisher_ = this->create_publisher<geometry_msgs::msg::Point>("/mb/gimbal_image_target_position", 10);

        // 启动串口和UDP读取线程
        serial_thread_ = std::thread([this]() { this->read_serial_data(); });
        udp_thread_ = std::thread([this]() { this->read_udp_data(); });
    }

    ~GimbalControlBridgeNode() {
        if (serial_thread_.joinable()) {
            serial_thread_.join();
        }
        if (udp_thread_.joinable()) {
            udp_thread_.join();
        }
    }

private:
    std::string udp_ip_;
    int udp_port_;
    std::string serial_port_name_;
    int baud_rate_;
    
    const int skydroid_packetlen_min = 12; // packet length not including the data segment

    boost::asio::io_context udp_ioc_;
    boost::asio::io_context serial_ioc_;

    udp::socket udp_socket_;
    udp::endpoint remote_endpoint_;
    udp::endpoint sender_endpoint_;
    boost::asio::serial_port serial_port_;

    std::string buffer_;  // 串口接收数据缓冲区
    std::mutex mtx_;

    std::thread serial_thread_;
    std::thread udp_thread_;

    rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr position_publisher_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr speed_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angle_subscriber_;

    // 处理速度控制消息
    void handle_speed_control(const geometry_msgs::msg::Twist::SharedPtr msg) {
        int yaw_speed = static_cast<int>(msg->angular.z * 2);  // 假设angular.z表示航向速度
        int pitch_speed = static_cast<int>(msg->angular.y * 2);  // 假设angular.y表示俯仰速度

        std::string command = generate_speed_command(yaw_speed, pitch_speed);
        send_command(command);
    }

    // 处理角度控制消息
    void handle_angle_control(const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
        if (msg->data.size() < 2) {
            RCLCPP_ERROR(this->get_logger(), "角度控制消息数据不完整");
            return;
        }
        int yaw_angle = static_cast<int>(msg->data[0] * 100);  // 假设第一个值为航向角度，单位0.01度
        int pitch_angle = static_cast<int>(msg->data[1] * 100);  // 假设第二个值为俯仰角度，单位0.01度

        std::string command = generate_angle_command(yaw_angle, pitch_angle);
        send_command(command);
    }

    // 生成速度控制命令
    std::string generate_speed_command(int yaw_speed, int pitch_speed) {
        const char* format_str = "%02X";

        const uint16_t yaw_speed_tmp = yaw_speed;

        char databuff[3] = {0};
        snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, yaw_speed_tmp);
        std::string yaw_speed_str(databuff);

        std::stringstream yaw_speed_ss;
        yaw_speed_ss << "#TPUG2wGSY" << yaw_speed_str;
        yaw_speed_ss << calculate_crc(yaw_speed_ss.str());
        
        const uint16_t pitch_speed_tmp = pitch_speed;
        snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, pitch_speed_tmp);
        std::string pitch_speed_str(databuff);

        std::stringstream pitch_speed_ss;
        pitch_speed_ss << "#TPUG2wGSP" << pitch_speed_str;
        pitch_speed_ss << calculate_crc(pitch_speed_ss.str());

        std::stringstream ss;
        ss << yaw_speed_ss.str() << pitch_speed_ss.str();

        return ss.str();
    }

    // 生成角度控制命令
    std::string generate_angle_command(int yaw_angle, int pitch_angle) {
        const char* format_str = "%04X%02X";
        const uint8_t speed = 99;
        const uint16_t yaw_angle_cd = yaw_angle;

        char databuff[7] = {0};
        snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, yaw_angle_cd, speed);
        std::string yaw_angle_str(databuff);

        std::stringstream yaw_angle_ss;
        yaw_angle_ss << "#TPUG6wGAY" << yaw_angle_str;
        yaw_angle_ss << calculate_crc(yaw_angle_ss.str());
        
        const uint16_t pitch_angle_cd = pitch_angle;
        snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, pitch_angle_cd, speed);
        std::string pitch_angle_str(databuff);

        std::stringstream pitch_angle_ss;
        pitch_angle_ss << "#TPUG6wGAP" << pitch_angle_str;
        pitch_angle_ss << calculate_crc(pitch_angle_ss.str());

        std::stringstream ss;
        ss << yaw_angle_ss.str() << pitch_angle_ss.str();

        return ss.str();
    }

    // 发送命令到串口或UDP
    void send_command(const std::string &command) {
        try {
            {
                std::lock_guard<std::mutex> lock(mtx_);

                udp_socket_.send_to(boost::asio::buffer(command), remote_endpoint_);
                RCLCPP_INFO(this->get_logger(), "发送指令至云台 (UDP): %s", command.c_str());
            }
        } catch (std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "通过UDP发送命令失败: %s", e.what());
        }
    }

    // 读取串口数据并透传到UDP
    void read_serial_data() {
        char data[256];
        while (rclcpp::ok()) {
            try {
                //std::size_t bytes_read = boost::asio::async_read(serial_port_, boost::asio::buffer(data, sizeof(data)));
                std::size_t bytes_read = serial_port_.read_some(boost::asio::buffer(data, sizeof(data)));
                buffer_ += std::string(data, bytes_read);

                // 处理串口接收的完整帧
                process_received_data();

#if 0
                std::string data_str(data, bytes_read);

                {
                    std::lock_guard<std::mutex> lock(mtx_);

                    // 透传到UDP                    
                    udp_socket_.send_to(boost::asio::buffer(data_str), remote_endpoint_);
                }
                
                RCLCPP_INFO(this->get_logger(), "从串口接收到数据: %s", data_str.c_str());
                RCLCPP_INFO(this->get_logger(), "已透传数据至UDP");
#endif
            } catch (std::exception& e) {
                RCLCPP_ERROR(this->get_logger(), "串口读取时出错: %s", e.what());
                buffer_.clear();
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    }

    // 处理接收的完整帧
    void process_received_data() {
        while (1) {
            // 查找帧头，例如假设帧头为 "#TP"
            std::size_t start1 = buffer_.find("#TP");
            std::size_t start2 = buffer_.find("#tp");
        
            std::size_t start = std::min(start1, start2);

            if (start != std::string::npos) {
                // 确保长度字段存在，假设长度字段在帧的第4字节
                if (buffer_.length() > start + skydroid_packetlen_min) {
                    // 获取长度字段
                    int data_length = hex_to_int(buffer_.substr(start + 5, 1));
                    int frame_length = data_length + skydroid_packetlen_min;

                    // RCLCPP_WARN(this->get_logger(), "start %d", start);
                    // RCLCPP_WARN(this->get_logger(), "data_length %d", data_length);
                    // RCLCPP_WARN(this->get_logger(), "frame_length %d", frame_length);
                    // RCLCPP_WARN(this->get_logger(), "buffer_: %s", buffer_.c_str());
                    // RCLCPP_WARN(this->get_logger(), "buffer_length: %d", buffer_.length());
                    
                    if (data_length <= 0 || data_length > 0x0F) {
                        buffer_.clear();
                        break;
                    }

                    // 确保接收到完整的帧
                    if (buffer_.length() >= start + frame_length) {
                        std::string frame = buffer_.substr(start, frame_length); // 提取完整帧

                        // 校验帧的有效性（例如CRC校验）
                        if (verify_frame(frame)) {
                            {
                                std::lock_guard<std::mutex> lock(mtx_);
                                
                                // 透传到UDP
                                udp_socket_.send_to(boost::asio::buffer(frame), remote_endpoint_);
                            }
                            
                            RCLCPP_INFO(this->get_logger(), "从串口接收到有效帧: %s", frame.c_str());
                            RCLCPP_INFO(this->get_logger(), "已透传数据至UDP");

                            // 检查是否为SIT标识位
                            if (frame.find("SIT") != std::string::npos) {
                                parse_and_publish_sit_data(frame);
                            }
                        } else {
                            RCLCPP_WARN(this->get_logger(), "收到的帧校验失败");
                        }

                        // 移除已处理的帧数据
                        buffer_.erase(0, start + frame_length);
                    } else {
                        break;
                    }
                } else {
                    break;
                }
            } else {
                break;
            }
        }
    }

    // 验证帧（例如通过CRC校验）
    bool verify_frame(const std::string &frame) {
        // 计算CRC并与帧中的CRC比较（假设CRC在最后两位）
        std::string data_without_crc = frame.substr(0, frame.length() - 2);
        std::string received_crc = frame.substr(frame.length() - 2, frame.length());
        std::string calculated_crc = calculate_crc(data_without_crc);

        // RCLCPP_INFO(this->get_logger(), "data_without_crc: %s", data_without_crc.c_str());
        // RCLCPP_INFO(this->get_logger(), "received_crc: %s", received_crc.c_str());
        // RCLCPP_INFO(this->get_logger(), "calculated_crc: %s", calculated_crc.c_str());

        return received_crc == calculated_crc;
    }

    // 解析SIT数据并发布ROS消息
    void parse_and_publish_sit_data(const std::string &data) {
        try {
            // 提取GOT指令中的X和Y坐标
            std::string x_hex = data.substr(data.find("SIT") + 3, 3);  // 提取X坐标部分
            std::string y_hex = data.substr(data.find("SIT") + 6, 3);  // 提取Y坐标部分

            int x_coord = hex_to_int(x_hex);
            int y_coord = hex_to_int(y_hex);

            RCLCPP_INFO(this->get_logger(), "解析得到坐标: X = %d, Y = %d", x_coord, y_coord);

            // 创建ROS消息并发布
            geometry_msgs::msg::Point point_msg;
            point_msg.x = static_cast<double>(x_coord);
            point_msg.y = static_cast<double>(y_coord);
            point_msg.z = 0.0;  // 可以根据需求设置Z轴

            position_publisher_->publish(point_msg);
        } catch (std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "解析GOT数据时出错: %s", e.what());
        }
    }

    // 读取UDP数据并透传到串口
    void read_udp_data() {
        char data[256];
        while (rclcpp::ok()) {
            try {
                std::size_t bytes_received = udp_socket_.receive_from(boost::asio::buffer(data, 256), sender_endpoint_);
                RCLCPP_INFO(this->get_logger(), "从UDP接收到数据: %s", std::string(data, bytes_received).c_str());

                // 透传到串口
                boost::asio::write(serial_port_, boost::asio::buffer(data, bytes_received));
                RCLCPP_INFO(this->get_logger(), "已透传数据至串口");
            } catch (std::exception& e) {
                RCLCPP_ERROR(this->get_logger(), "UDP读取或串口发送时出错: %s", e.what());
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    }

    // 打开串口
    void open_serial_port() {
        try {
            serial_port_.open(serial_port_name_);
            serial_port_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
            RCLCPP_INFO(this->get_logger(), "成功打开串口: %s", serial_port_name_.c_str());
        } catch (std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "打开串口失败: %s", e.what());
        }
    }

    // 计算CRC校验和
    std::string calculate_crc(const std::string &data) {
        char crc = 0;
        for (char c : data) {
            crc += c;
        }
        std::stringstream ss;
        ss << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast<int>(crc);
        return ss.str();
    }

    // 将整数转换为十六进制字符串
    std::string int_to_hex_string(int value, int width) {
        std::stringstream ss;
        ss << std::hex << std::setw(width) << std::setfill('0') << (value & 0xFFFF);
        return ss.str();
    }

    // 将十六进制字符串转换为整数
    int hex_to_int(const std::string &hex_str) {
        int result;
        std::stringstream ss;
        ss << std::hex << hex_str;
        ss >> result;
        return result;
    }
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GimbalControlBridgeNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
